#include "cfar_detector.h"

// 检测函数
std::vector<int> CFARDetector::detect(const std::vector<float> &power_spectrum)
{
    std::vector<int> detections;
    int n = power_spectrum.size();

    for (int i = 0; i < n; ++i)
    {
        float noise_power = 0.0f;

        switch (cfar_type)
        {
        case CA_CFAR:
            noise_power = computeCACFARNoise(power_spectrum, i);
            break;
        case GO_CFAR:
            noise_power = computeGOCFARNoise(power_spectrum, i);
            break;
        case SO_CFAR:
            noise_power = computeSOCFARNoise(power_spectrum, i);
            break;
        case OS_CFAR:
            noise_power = computeOSCFARNoise(power_spectrum, i);
            break;
        }

        // 应用阈值
        if (noise_power > 0 && power_spectrum[i] > threshold_factor * noise_power)
        {
            detections.push_back(i);
        }
    }

    return detections;
}

// 单元平均CFAR
float CFARDetector::computeCACFARNoise(const std::vector<float> &power_spectrum, int test_cell)
{
    int left_start = std::max(0, test_cell - guard_cells - training_cells);
    int left_end = std::max(0, test_cell - guard_cells);

    int right_start = std::min(static_cast<int>(power_spectrum.size()), test_cell + guard_cells + 1);
    int right_end = std::min(static_cast<int>(power_spectrum.size()), test_cell + guard_cells + training_cells + 1);

    float noise_power = 0.0f;
    int count = 0;

    for (int j = left_start; j < left_end; ++j)
    {
        noise_power += power_spectrum[j];
        count++;
    }

    for (int j = right_start; j < right_end; ++j)
    {
        noise_power += power_spectrum[j];
        count++;
    }

    return count > 0 ? noise_power / count : 0.0f;
}

// 最大选择CFAR
float CFARDetector::computeGOCFARNoise(const std::vector<float> &power_spectrum, int test_cell)
{
    int left_start = std::max(0, test_cell - guard_cells - training_cells);
    int left_end = std::max(0, test_cell - guard_cells);

    int right_start = std::min(static_cast<int>(power_spectrum.size()), test_cell + guard_cells + 1);
    int right_end = std::min(static_cast<int>(power_spectrum.size()), test_cell + guard_cells + training_cells + 1);

    float left_noise = 0.0f;
    float right_noise = 0.0f;
    int left_count = 0;
    int right_count = 0;

    for (int j = left_start; j < left_end; ++j)
    {
        left_noise += power_spectrum[j];
        left_count++;
    }

    for (int j = right_start; j < right_end; ++j)
    {
        right_noise += power_spectrum[j];
        right_count++;
    }

    left_noise = left_count > 0 ? left_noise / left_count : 0.0f;
    right_noise = right_count > 0 ? right_noise / right_count : 0.0f;

    return std::max(left_noise, right_noise);
}

// 最小选择CFAR
float CFARDetector::computeSOCFARNoise(const std::vector<float> &power_spectrum, int test_cell)
{
    int left_start = std::max(0, test_cell - guard_cells - training_cells);
    int left_end = std::max(0, test_cell - guard_cells);

    int right_start = std::min(static_cast<int>(power_spectrum.size()), test_cell + guard_cells + 1);
    int right_end = std::min(static_cast<int>(power_spectrum.size()), test_cell + guard_cells + training_cells + 1);

    float left_noise = 0.0f;
    float right_noise = 0.0f;
    int left_count = 0;
    int right_count = 0;

    for (int j = left_start; j < left_end; ++j)
    {
        left_noise += power_spectrum[j];
        left_count++;
    }

    for (int j = right_start; j < right_end; ++j)
    {
        right_noise += power_spectrum[j];
        right_count++;
    }

    left_noise = left_count > 0 ? left_noise / left_count : 0.0f;
    right_noise = right_count > 0 ? right_noise / right_count : 0.0f;

    return std::min(left_noise, right_noise);
}

// 有序统计CFAR
float CFARDetector::computeOSCFARNoise(const std::vector<float> &power_spectrum, int test_cell)
{
    int start = std::max(0, test_cell - guard_cells - training_cells);
    int end = std::min(static_cast<int>(power_spectrum.size()), test_cell + guard_cells + training_cells + 1);

    std::vector<float> window;
    for (int j = start; j < end; ++j)
    {
        if (j < test_cell - guard_cells || j > test_cell + guard_cells)
        {
            window.push_back(power_spectrum[j]);
        }
    }

    if (window.empty())
        return 0.0f;

    // 排序并选择第k个值
    int k = static_cast<int>(window.size() * 0.75f); // 选择75%位置的值
    std::nth_element(window.begin(), window.begin() + k, window.end());

    return window[k];
}